#include <rclcpp/rclcpp.hpp>
#include <queue>
#include <chrono>
#include <iostream>
#include <fstream>
#include <thread>
#include <std_msgs/msg/bool.hpp>
#include <regex>

class GPIODriver : public rclcpp::Node
{
public:
    GPIODriver() : Node("gpio_driver")
    {
        RCLCPP_WARN(this->get_logger(), "starting GPIO driver node");

        gpio_line_ = 132;

        const std::string directory = get_ros2_pkg_prefix("can_motor_driver");

        const std::string export_cmd = directory + "/src/gpio_libs/gpio_lib export " + std::to_string(gpio_line_);

        // RCLCPP_INFO(this->get_logger(), "cmd:%s", export_cmd.c_str());

        system(export_cmd.c_str());

        timer_ = this->create_wall_timer(
            std::chrono::duration<double>(1.0/100.0), std::bind(&GPIODriver::timer_callback, this));

        switch_pub = this->create_publisher<std_msgs::msg::Bool>("/gpio_driver/switch_state", 1000);
        switch_trigger_pub = this->create_publisher<std_msgs::msg::Bool>("/gpio_driver/switch_trigger", 1000);


    }
private:
    std::string replaceInstallWithSrc(const std::string& str) {
        std::string result = str;
        std::string toReplace = "install";
        std::string replaceWith = "src";
        
        size_t pos = 0;
        while ((pos = result.find(toReplace, pos)) != std::string::npos) {
            result.replace(pos, toReplace.length(), replaceWith);
            pos += replaceWith.length(); // Move past the replaced substring
        }
        
        return result;
    }


    std::string get_ros2_pkg_prefix(const std::string& pkg_name) {
        std::string command = "ros2 pkg prefix " + pkg_name;
        std::array<char, 128> buffer;
        std::string result;
        std::unique_ptr<FILE, decltype(&pclose)> pipe(popen(command.c_str(), "r"), pclose);

        if (!pipe) {
            std::cerr << "Failed to run command." << std::endl;
            return "";
        }
        while (fgets(buffer.data(), buffer.size(), pipe.get()) != nullptr) {
            result += buffer.data();
        }

        result = replaceInstallWithSrc(result);

        result.erase(std::remove(result.begin(), result.end(), '\n'), result.end());
        // result.erase(std::remove(result.begin(), result.end(), ' '), result.end());
        // RCLCPP_INFO(this->get_logger(), "pkg prefix: %s", result.c_str());

        return result;
    }

    void timer_callback()
    {
        std::ifstream value_file("/sys/class/gpio/gpio" + std::to_string(gpio_line_) + "/value");
        int value;
        value_file >> value;
        value_file.close();

        // if(last_switch_value == true && value == false) // from untouch to touch
        if(last_switch_value == false && value == true) // from untouch to touch
        {
            std_msgs::msg::Bool msg;
            msg.data = true;
            switch_trigger_pub->publish(msg);
        }

        else if(last_switch_value == true && value == false)// from touch to untouch
        // else if(last_switch_value == false && value == true)// from touch to untouch
        {
            std_msgs::msg::Bool msg;
            msg.data = false;
            switch_trigger_pub->publish(msg);
        }
        last_switch_value = value;

        std_msgs::msg::Bool msg;
        msg.data = value;
        switch_pub->publish(msg);

        // RCLCPP_INFO(this->get_logger(), "GPIO1_A0 (GPIO %d) value: %d", gpio_line_, value);
    }

    int gpio_line_;
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr switch_pub;
    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr switch_trigger_pub;

    bool last_switch_value = false; //如果一开始就是触发状态那么就是初始值如果是fasle -读取到true 则触发 否则
    // 如果是true -读取到true 则无法触发会导致碰到限位但是没有被触发再回收就会扭矩过大或者造成别的问题   


};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<GPIODriver>());
    rclcpp::shutdown();
    return 0;
}
